/*******************************************************************************
* This file implements the data logging functions to the serial flash.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include "datalog.h"
#include "hal_adc.h"



/*******************************************************************************
* PRIVATE CONSTANT DEFINITION                                                  *
*******************************************************************************/

#define LOG_INDEX_BASE_ADD	0x08000
#define LOG_DATA_BASE_ADD	0x09000



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

static char pcUartWriteBuffer[200];



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static struct LOG_INDEX prv_xNewLogIndex(unsigned char ucLogId);
static unsigned char prv_ucNewLogId(void);



/*******************************************************************************
* PRIVATE FUNCTION: prv_ucNewLogId
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ New log ID for the current entry.
*
* DESCRIPTIONS:
* Get a new log ID.
*
*******************************************************************************/
static unsigned char prv_ucNewLogId(void)
{
	struct LOG_INDEX xLogIndex;
	unsigned long ulIndexAdd = LOG_INDEX_BASE_ADD;
	unsigned char ucLogId = 0;
	
	// Loop until we found an empty space.
	do {
		// Read an Index from the serial flash.
		vSFlashRead(ulIndexAdd, sizeof(xLogIndex), (unsigned char*)&xLogIndex);
		
		// Increase the index address.
		ulIndexAdd += sizeof(xLogIndex);
		
		// Get the biggest log ID.
		if ((xLogIndex.ucLogId != 0xff) && xLogIndex.ucLogId >= ucLogId) {
			ucLogId = xLogIndex.ucLogId + 1;
		}	
	} while (xLogIndex.ucLogId != 0xff);
	
	return ucLogId;
}	



/*******************************************************************************
* PRIVATE FUNCTION: prv_xNewLogIndex
*
* PARAMETERS:
* ~ ucLogId	- Log ID for the new index.
*
* RETURN:
* ~ New log index.
* ~ Return an empty index if the memory is full.
*
* DESCRIPTIONS:
* Create a new log index in the serial flash.
*
*******************************************************************************/
static struct LOG_INDEX prv_xNewLogIndex(unsigned char ucLogId)
{
	struct LOG_INDEX xLogIndex;
	static unsigned long ulIndexAdd = LOG_INDEX_BASE_ADD;
	unsigned char ucAddOffsetMsb = 0;
	
	// Search for empty space.
	// Loop until we found an empty space.
	do {
		// Read an Index from the serial flash.
		vSFlashRead(ulIndexAdd, sizeof(xLogIndex), (unsigned char*)&xLogIndex);
		
		// Copy and increase the log address offset MSB if it's a valid log index.
		if (xLogIndex.ucLogId != 0xff) {
			ucAddOffsetMsb = xLogIndex.ucAddOffsetMsb + 1;
			
			// Also increase the index address.
			ulIndexAdd += sizeof(xLogIndex);
		}	
	} while (xLogIndex.ucLogId != 0xff);
	
	// Return blank index if the memory is full.
	if ((((unsigned long)ucAddOffsetMsb << 12) + LOG_DATA_BASE_ADD) > 0xff000) {
		xLogIndex.ucLogId = 0xff;
		xLogIndex.ucAddOffsetMsb = 0xff;
	}	
	// Else, copy the data of the new index.
	else {
		xLogIndex.ucLogId = ucLogId;
		xLogIndex.ucAddOffsetMsb = ucAddOffsetMsb;
	}
	
	// Write the new index to the serial flash.
	vSFlashWrite(ulIndexAdd, sizeof(xLogIndex), (unsigned char*)&xLogIndex);
	
	return xLogIndex;
}	



/*******************************************************************************
* TASK: taskDataLog
*
* DESCRIPTIONS:
* This task logs the flight data to the serial flash.
*
*******************************************************************************/
portTASK_FUNCTION(taskDataLog, pvParameters)
{
	unsigned long ulAddress;
	struct LOG_INDEX xLogIndex;
	struct LOG_DESCRIPTOR xLogDescriptor;
	struct LOG_DATA xLogData;
	
	
	
	// Suspend task if the push button is not pressed at startup.
	if (PUSH_BUTTON == 1) {
		vTaskSuspend(xTaskGetCurrentTaskHandle());
	}	
	
	
	
	// Only start logging when the system is ready and there is GPS fix.
	while ((xSystemState.bDcmReady == 0) ||
		   (xSystemState.bAltitudeReady == 0) ||
		   (xSystemState.bControlReady == 0) ||
		   (xGetGpsData().ucFixStatus == 0)) {
		// Let other task to run while waiting.
		// We check every 200ms.
		vTaskDelay(configTICK_RATE_HZ / 5);	
	}	
	
	
	
	// Create the descriptor.
	xLogDescriptor.xDateTime = xGetGpsData().xDateTime;
	xLogDescriptor.ucLogId = prv_ucNewLogId();
	
	// Copy the log ID to the log data.
	xLogData.ucLogId = xLogDescriptor.ucLogId;
	
	// Suspend the task if the Log ID is invalid.
	if (xLogDescriptor.ucLogId == 0xff) {
		xSystemError.bLogError = 1;
		vTaskSuspend(xTaskGetCurrentTaskHandle());
	}
	
	
	
	// Create a new log index.
	xLogIndex = prv_xNewLogIndex(xLogDescriptor.ucLogId);
	
	// Suspend the task if the memory is full.
	if (xLogIndex.ucLogId == 0xff) {
		xSystemError.bLogError = 1;
		vTaskSuspend(xTaskGetCurrentTaskHandle());
	}
	
	// Get the log address.
	ulAddress = ((unsigned long)xLogIndex.ucAddOffsetMsb << 12) + LOG_DATA_BASE_ADD;
	
	
	// Write the descriptor to the serial flash.
	vSFlashWrite(ulAddress, sizeof(xLogDescriptor), (unsigned char*)&xLogDescriptor);
	ulAddress += sizeof(xLogDescriptor);
	
	
	
	// Indicate the logging is running.
	xSystemState.bLogRunning = 1;
	
	
	
	// We need to initialise xLastFlashTime prior to the first call to vTaskDelayUntil().
	portTickType xLastFlashTime = xTaskGetTickCount();
	
	while (1) {

		// Get the GPS and attitude data.
		struct GPS_DATA xGpsData = xGetGpsData();
		struct ATTITUDE xAttitude = xGetAttitude();
		
		
		
		// Prepare the data.
		xLogData.ucMode = (unsigned char)eGetControlMode();
		
		xLogData.ucFixStatus = xGpsData.ucFixStatus;
		xLogData.fLatitude = xGpsData.fLatitude;
		xLogData.fLongitude = xGpsData.fLongitude;
		xLogData.uiGpsSpeed = (unsigned int)(xGpsData.fGroundSpeed * MS_TO_KMH);
		xLogData.uiGpsHeading = (unsigned int)xGpsData.fGroundCourse;
		xLogData.siGpsAltitude = (signed int)xGpsData.fAltitude;
		
		xLogData.uiAirspeed = (unsigned int)(fGetAirspeed() * MS_TO_KMH);
		xLogData.uiMagHeading = (unsigned int)(xAttitude.fMagHeading * RAD_TO_DEG);
		xLogData.siPressureAltitude = (signed int)fGetPressureAltitude();
		
		xLogData.siRoll = (signed int)(xAttitude.fRoll * RAD_TO_DEG * 10.0f);
		xLogData.siPitch = (signed int)(xAttitude.fPitch * RAD_TO_DEG * 10.0f);
		xLogData.siYaw = (signed int)(xAttitude.fYaw * RAD_TO_DEG * 10.0f);
		
		xLogData.uiHeadingSetpoint = (unsigned int)(fGetHeadingSetpoint() * RAD_TO_DEG * 10.0f);
		
		xLogData.uiServo1 = uiReadAdc(1);
		xLogData.uiServo2 = OC2RS;
		xLogData.uiServo3 = OC3RS;
		xLogData.uiServo4 = OC4RS;
		xLogData.uiServo5 = OC5RS;
		xLogData.uiServo6 = OC6RS;
		
		xLogData.ucHour = xGpsData.xDateTime.ucHour;
		xLogData.ucMinute = xGpsData.xDateTime.ucMinute;
		xLogData.ucSecond = xGpsData.xDateTime.ucSecond;	
		
		
		
		// Write to the serial flash.
		vSFlashWrite(ulAddress, sizeof(xLogData), (unsigned char*)&xLogData);
		
		// Increase the address,
		// Create a new index if we cross the 4KB boundary.
		unsigned char ucOldMemBlock = (unsigned char)(ulAddress >> 12);
		ulAddress += sizeof(xLogData);
		
		if ((unsigned char)(ulAddress >> 12) > ucOldMemBlock) {
			xLogIndex = prv_xNewLogIndex(xLogData.ucLogId);
			
			// Suspend the task if the memory is full.
			if (xLogIndex.ucLogId == 0xff) {
				xSystemError.bLogError = 1;
				xSystemState.bLogRunning = 0;
				vTaskSuspend(xTaskGetCurrentTaskHandle());
			}
		}	
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiDataLog = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiDataLog < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}
		
		// Delay for 200ms (5Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 5);
	}	
}	



/*******************************************************************************
* PUBLIC FUNCTION: vMemoryDump
*
* PARAMETERS:
* ~ ulStartAdd	- Start address.
* ~ ulEndAdd	- End address.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Send the content of the memory to PC.
* This is a blocking function and should not be called from ISR.
*
*******************************************************************************/
void vMemoryDump(unsigned long ulStartAdd, unsigned long ulEndAdd)
{
	unsigned char pucBuffer[16];
	
	// Make sure the start and end address is aligned to the boundary of 16 bytes.
	ulStartAdd &= 0xffff0;
	ulEndAdd &= 0xffff0;
	
	// Display the title.
	sprintf(pcUartWriteBuffer, "Memory Dump:\r\n");
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 10ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 100);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
	
	do {
		// Read 16 bytes of data.
		vSFlashRead(ulStartAdd, 16, pucBuffer);
		
		// Send the address for current line.
		sprintf(pcUartWriteBuffer, "\r\n0x%05lX:", ulStartAdd);
		while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
			// If we are calling from task, delay for 10ms to allow other task to run.
			if (xTaskGetCurrentTaskHandle() != NULL) {
				vTaskDelay(configTICK_RATE_HZ / 100);
			}	
		}	
		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
		
		// Send the content of the memory for current line.
		unsigned char i;
		for (i = 0; i < 16; i++) {
			sprintf(pcUartWriteBuffer, " %02X", pucBuffer[i]);
			while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
				// If we are calling from task, delay for 10ms to allow other task to run.
				if (xTaskGetCurrentTaskHandle() != NULL) {
					vTaskDelay(configTICK_RATE_HZ / 100);
				}	
			}	
			ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
		}	
		
		// Increase 16 bytes.
		ulStartAdd += 0x10;
	} while (ulStartAdd <= ulEndAdd);	
	
	
	
	// Indicate that we have finished printing.
	sprintf(pcUartWriteBuffer, "\r\n\r\n---END---\r\n\r\n\r\n");
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 10ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 100);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
}	



/*******************************************************************************
* PUBLIC FUNCTION: vPrintLogData
*
* PARAMETERS:
* ~ ucLogId	- Log ID of the data we want to print.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Send the content of the log data to PC.
* The data will be sent as CSV format.
* This is a blocking function and should not be called from ISR.
*
*******************************************************************************/
void vPrintLogData(unsigned char ucLogId)
{
	unsigned long ulIndexAdd = LOG_INDEX_BASE_ADD;
	unsigned long ulLogDataAdd;
	struct LOG_DATA xLogData;
	struct LOG_INDEX xLogIndex;
	struct LOG_DESCRIPTOR xLogDescriptor;
	
	// Get the log index for the given ID.
	// Loop until we found the index or we get an empty index.
	do {
		// Read an Index from the serial flash.
		vSFlashRead(ulIndexAdd, sizeof(xLogIndex), (unsigned char*)&xLogIndex);
		
		// Increase the index address.
		ulIndexAdd += sizeof(xLogIndex);
	} while ((xLogIndex.ucLogId != 0xff) && (xLogIndex.ucLogId != ucLogId));
	
	// Print error message and exit if the log index is not found.
	if (xLogIndex.ucLogId != ucLogId) {
		sprintf(pcUartWriteBuffer, "Log ID not found!\r\n\r\n");
		while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
			// If we are calling from task, delay for 10ms to allow other task to run.
			if (xTaskGetCurrentTaskHandle() != NULL) {
				vTaskDelay(configTICK_RATE_HZ / 100);
			}	
		}	
		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
		
		return;
	}	
	
	
	
	// Get the start address of the log data.
	ulLogDataAdd = ((unsigned long)xLogIndex.ucAddOffsetMsb << 12) + LOG_DATA_BASE_ADD;
	
	
	
	// Get the descriptor.
	vSFlashRead(ulLogDataAdd, sizeof(xLogDescriptor), (unsigned char*)&xLogDescriptor);
	
	// Increase the address,
	ulLogDataAdd += sizeof(xLogDescriptor);
	
	
	
	// Display the descriptor info.
	sprintf(pcUartWriteBuffer, "Date: %02u/%02u/20%02u\r\n", xLogDescriptor.xDateTime.ucDate,
														 xLogDescriptor.xDateTime.ucMonth,
														 xLogDescriptor.xDateTime.ucYear);
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 10ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 100);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
	
	
	
	// Display the header.
	sprintf(pcUartWriteBuffer, "Time,"
							   "Mode,"
							   "GPS Fix,"
							   "Latitude,"
							   "Longitude,"
							   "GPS Speed,"
							   "GPS Heading,"
							   "GPS Altitude,"
							   "Airspeed,"
							   "Mag Heading,"
							   "Pressure Altitude,"
							   "Roll,"
							   "Pitch,"
							   "Yaw,"
							   "Heading Setpoint,"
							   "Servo 1,"
							   "Servo 2,"
							   "Servo 3,"
							   "Servo 4,"
							   "Servo 5,"
							   "Servo 6"
							   "\r\n");
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 10ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 100);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
	
	
	// Loop until we have printed all the data.
	do {
		// Read from the serial flash.
		vSFlashRead(ulLogDataAdd, sizeof(xLogData), (unsigned char*)&xLogData);
		
		// Increase the address,
		ulLogDataAdd += sizeof(xLogData);
		
		// Print the data if the log ID is same.
		if (xLogData.ucLogId == ucLogId) {
			
			sprintf(pcUartWriteBuffer, "%02u:%02u:%02u,"	// Time
									   "%u,"				// Mode
									   "%u,"				// GPS Fix
									   "%+.6f,"				// Latitude
									   "%+.6f,"				// Longitude
									   "%3u,"				// GPS Speed
									   "%3u,"				// GPS Heading
									   "%+4d,"				// GPS Altitude
									   "%3u,"				// Airspeed
									   "%3u,"				// Mag Heading
									   "%+4d,"				// Pressure Altitude
									   "%+.1f,"				// Roll
									   "%+.1f,"				// Pitch
									   "%+.1f,"				// Yaw
									   "%+.1f,"				// Heading Setpoint
									   "%5u,"				// Servo 1
									   "%5u,"				// Servo 2
									   "%5u,"				// Servo 3
									   "%5u,"				// Servo 4
									   "%5u,"				// Servo 5
									   "%5u"				// Servo 6
									   "\r\n",
									   xLogData.ucHour, xLogData.ucMinute, xLogData.ucSecond,
									   xLogData.ucMode,
									   xLogData.ucFixStatus,
									   (double)xLogData.fLatitude,
									   (double)xLogData.fLongitude,
									   xLogData.uiGpsSpeed,
									   xLogData.uiGpsHeading,
									   xLogData.siGpsAltitude,
									   xLogData.uiAirspeed,
									   xLogData.uiMagHeading,
									   xLogData.siPressureAltitude,
									   (double)((float)xLogData.siRoll / 10.0f),
									   (double)((float)xLogData.siPitch / 10.0f),
									   (double)((float)xLogData.siYaw / 10.0f),
									   (double)((float)xLogData.uiHeadingSetpoint / 10.0f),
									   xLogData.uiServo1,
									   xLogData.uiServo2,
									   xLogData.uiServo3,
									   xLogData.uiServo4,
									   xLogData.uiServo5,
									   xLogData.uiServo6
									   );
			while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
				// If we are calling from task, delay for 10ms to allow other task to run.
				if (xTaskGetCurrentTaskHandle() != NULL) {
					vTaskDelay(configTICK_RATE_HZ / 100);
				}	
			}	
			ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
			
		}	
		
	} while (xLogData.ucLogId == ucLogId);	
	
	
	
	// Indicate that we have finished printing.
	sprintf(pcUartWriteBuffer, "\r\n---END---\r\n\r\n\r\n");
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 10ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 100);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
}	



/*******************************************************************************
* PUBLIC FUNCTION: vListLog
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* List out all the log entry together with it's data and time.
* This is a blocking function and should not be called from ISR.
*
*******************************************************************************/
void vListLog(void)
{
	unsigned long ulIndexAdd = LOG_INDEX_BASE_ADD;
	unsigned long ulLogDataAdd;
	struct LOG_INDEX xLogIndex;
	struct LOG_DESCRIPTOR xLogDescriptor;
	unsigned char ucLogId = 0xff;
	
	
	
	// Send the title
	sprintf(pcUartWriteBuffer, "Log List:\r\n\r\n");
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 10ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 100);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
	
	
	
	// Read the log index
	// Loop until we get an empty index.
	do {
		// Read an Index from the serial flash.
		vSFlashRead(ulIndexAdd, sizeof(xLogIndex), (unsigned char*)&xLogIndex);
		
		// Increase the index address.
		ulIndexAdd += sizeof(xLogIndex);
		
		
		// If this index is not empty and the log ID is not printed yet.
		if ((xLogIndex.ucLogId != 0xff) && (xLogIndex.ucLogId != ucLogId)) {
			// Recorde the current log ID.
			ucLogId = xLogIndex.ucLogId;
			
			// Get the descriptor.
			ulLogDataAdd = ((unsigned long)xLogIndex.ucAddOffsetMsb << 12) + LOG_DATA_BASE_ADD;
			vSFlashRead(ulLogDataAdd, sizeof(xLogDescriptor), (unsigned char*)&xLogDescriptor);
			
			// Display the descriptor info.
			sprintf(pcUartWriteBuffer, "%u: %02u/%02u/20%02u %02u:%02u:%02u\r\n",
					ucLogId,
					xLogDescriptor.xDateTime.ucDate, xLogDescriptor.xDateTime.ucMonth, xLogDescriptor.xDateTime.ucYear,
					xLogDescriptor.xDateTime.ucHour, xLogDescriptor.xDateTime.ucMinute, xLogDescriptor.xDateTime.ucSecond);
			
			while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
				// If we are calling from task, delay for 10ms to allow other task to run.
				if (xTaskGetCurrentTaskHandle() != NULL) {
					vTaskDelay(configTICK_RATE_HZ / 100);
				}	
			}	
			ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
		}	
		
		
	} while (xLogIndex.ucLogId != 0xff);
	
	
	
	// Indicate that we have finished printing.
	sprintf(pcUartWriteBuffer, "\r\n---END---\r\n\r\n\r\n");
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 10ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 100);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
}	
